import numpy as np
import matplotlib.pyplot as plt

# -------------------------- 1. 核心参数配置（聚焦时延变化） --------------------------
c = 3e8  # 光速(m/s)，用于计算传播时延
d0 = 100  # 初始距离(m)
v = 30     # 距离变远的速度(m/s)，模拟平缓变化（1m/s≈3.6km/h，符合战术场景低速移动）
sim_time = 100  # 仿真总时间(s)
dt = 1    # 时间步长(s)，每1秒更新一次时延
num_steps = int(sim_time / dt)  # 总步数

# 卡尔曼滤波参数（适配时延缓慢变化场景）
# 状态x：[当前真实时延(μs)]（将时延视为需要跟踪的状态，而非噪声）
x_est = 0.0  # 初始状态估计（初始时延的估计值）
P_est = 0.1  # 初始状态误差协方差（表示对初始估计的不确定度）

# 过程噪声：模拟时延变化的微小随机波动（非时延本身，如空气折射率变化）
Q = 0.01  # 过程噪声协方差（小值，因时延平缓变化）

# 测量噪声：模拟时延测量中的随机干扰（如多径、射频噪声）
R = 0.1   # 测量噪声协方差（小值，模拟高精度测量）

# -------------------------- 2. 生成“真实时延”与“带噪测量时延” --------------------------
# 数组初始化
true_distance = np.zeros(num_steps)  # 真实距离序列
true_tau = np.zeros(num_steps)       # 真实时延序列（距离→时延：tau = d/c，转换为μs）
measured_tau = np.zeros(num_steps)   # 带噪测量时延序列
kalman_est_tau = np.zeros(num_steps) # 卡尔曼滤波估计时延序列
compensation_error = np.zeros(num_steps) # 补偿误差（真实时延 - 滤波估计时延）

# 生成数据（每步距离递增，时延递增）
for t in range(num_steps):
    # 1. 真实距离：线性递增（d = d0 + v*t）
    true_distance[t] = d0 + v * t * dt  # t*dt是当前时间（s）
    
    # 2. 真实时延：tau = 距离/光速，转换为微秒（1e6）
    true_tau[t] = (true_distance[t] / c) * 1e6  # 单位：μs
    
    # 3. 带噪测量时延：真实时延 + 高斯测量噪声（模拟实际测量干扰）
    measured_noise = np.random.normal(0, np.sqrt(R))  # 测量噪声
    measured_tau[t] = true_tau[t] + measured_noise

# -------------------------- 3. 卡尔曼滤波处理时延递增（核心步骤） --------------------------
for t in range(num_steps):
    # -------------------------- 3.1 预测步骤：预判时延变化趋势
    # 状态预测：时延缓慢递增（真实时延 = 上一步真实时延 + 时延增量）
    # 因距离线性递增，时延增量固定：delta_tau = (v*dt)/c *1e6（每步时延增加量）
    delta_tau = (v * dt / c) * 1e6  # 每步时延递增的固定值（μs）
    x_pred = x_est + delta_tau  # 预测当前时延（基于上一步估计 + 固定增量）
    
    # 误差协方差预测：积累过程噪声
    P_pred = P_est + Q

    # -------------------------- 3.2 更新步骤：用带噪测量值修正预测
    # 计算卡尔曼增益（平衡预测与测量的信任度）
    K = P_pred / (P_pred + R)  # 增益越大，越信任测量值；越小，越信任预测值
    
    # 状态更新：用测量值修正预测值（跟踪真实时延）
    x_est = x_pred + K * (measured_tau[t] - x_pred)
    
    # 误差协方差更新：更新估计不确定度
    P_est = (1 - K) * P_pred

    # -------------------------- 3.3 记录结果
    kalman_est_tau[t] = x_est  # 保存滤波估计的时延
    compensation_error[t] = true_tau[t] - x_est  # 补偿误差（越小表示补偿越准）

# -------------------------- 4. 结果可视化（展示滤波效果） --------------------------
plt.figure(figsize=(12, 8))

# 子图1：真实距离与真实时延的关系（验证时延随距离递增）
plt.subplot(2, 1, 1)
plt.plot(true_distance, true_tau, 'b-', linewidth=2, label='True Delay(随距离递增)')
plt.xlabel('Distance (m)')
plt.ylabel('Delay (μs)')
plt.title('Delay increase')
plt.legend()
plt.grid(True)

# 子图2：真实时延、带噪测量时延、滤波估计时延对比（核心展示）
plt.subplot(2, 1, 2)
plt.plot(range(num_steps), true_tau, 'b-', linewidth=2, label='True Delay')
plt.plot(range(num_steps), measured_tau, 'r--', alpha=0.6, linewidth=1.5, label='Measure Delay')
plt.plot(range(num_steps), kalman_est_tau, 'g-', linewidth=2, label='Kalman Est Delay')
plt.xlabel('Step (1s)')
plt.ylabel('Delay (μs)')
plt.title('Kalman')
plt.legend()
plt.grid(True)

# 补充：补偿误差图（可选，展示补偿精度）
plt.figure(figsize=(12, 4))
plt.plot(range(num_steps), compensation_error, 'k-', linewidth=1.5)
plt.axhline(y=0, color='r', linestyle='--', alpha=0.8, label='No error(0)')
plt.xlabel('Step (1s)')
plt.ylabel('Error (True - Est) (μs)')
plt.title('compensation Error')
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.show()

# -------------------------- 5. 关键结果打印（量化效果） --------------------------
print(f"初始距离：{d0} m → 初始真实时延：{true_tau[0]:.6f} μs")
print(f"最终距离：{true_distance[-1]:.1f} m → 最终真实时延：{true_tau[-1]:.6f} μs")
print(f"时延总增量：{true_tau[-1] - true_tau[0]:.6f} μs（由距离递增导致）")
print(f"平均补偿误差：{np.mean(np.abs(compensation_error)):.6f} μs（越小表示补偿越准）")
print(f"最大补偿误差：{np.max(np.abs(compensation_error)):.6f} μs")